#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <chrono>
#include <iostream>
#include <stdlib.h>

class RTSPStreamer : public rclcpp::Node
{
public:
    RTSPStreamer()
    : Node("rtsp_streamer_node")
    {
        // 创建发布器
        image_pub_ = this->create_publisher<sensor_msgs::msg::Image>("/mb/video_stream_raw", 10);

        // RTSP 流 URL
        // 可以通过参数传递，也可以硬编码
        this->declare_parameter<std::string>("rtsp_url", "rtsp://127.0.0.1:8554/test");
        this->get_parameter("rtsp_url", rtsp_url_);

#if 0
        std::stringstream ss;

        //ss << "rtspsrc location=" << rtsp_url_ << " ! short-header=true ! latency=0 ! rtph264depay ! h264parse ! avdec_h264 ! decodebin ! videoconvert ! appsink";
        //ss << "rtspsrc location=" << rtsp_url_ << " ! nvh264dec ! nvh265dec ! omxh264dec ! omxh265dec ! rtph265depay ! h265parse ! avdec_h265 ! rtph264depay ! h264parse ! avdec_h264 ! short-header=true ! queue ! latency=0 ! decodebin ! videoconvert ! appsink";
        ss << "rtspsrc location=" << rtsp_url_ << " latency=0 ! queue ! avdec_h265 ! rtph264depay ! h264parse ! avdec_h264 ! decodebin ! videoconvert ! appsink";
        std::cout << ss.str() << std::endl;
        
        //std::cout << cv::getBuildInformation() << std::endl;

        // 打开 RTSP 流
        cap_.open(ss.str(), cv::CAP_GSTREAMER);
#else
        // std::cout << cv::getBuildInformation() << std::endl;

        char env[] = "OPENCV_FFMPEG_CAPTURE_OPTIONS=protocol_whitelist;file,rtp,udp,tcp";
        int rc = putenv(env);
        if (rc != 0) {
            std::cout << "Could not set environment variables" << std::endl;
        }
        
        cap_.open(rtsp_url_, cv::CAP_FFMPEG); // cv::CAP_FFMPEG
        
        //std::string std_str1(cap_.getBackendName());
        //std::cout << "[BackendName]: " << std_str1 << std::endl;
#endif

        // 设置缓冲区大小，可以减少延迟
        cap_.set(cv::CAP_PROP_BUFFERSIZE, 3); // 设置缓冲帧的数量

        if (!cap_.isOpened()) {
            RCLCPP_ERROR(this->get_logger(), "无法打开 RTSP 流: %s", rtsp_url_.c_str());
            rclcpp::shutdown();
            return;
        }

        RCLCPP_INFO(this->get_logger(), "成功连接到 RTSP 流: %s", rtsp_url_.c_str());

        // 设置定时器，控制帧率
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(20),  // 大约 50 FPS
            std::bind(&RTSPStreamer::publish_frame, this));
    }

    ~RTSPStreamer()
    {
        if (cap_.isOpened()) {
            cap_.release();
        }
    }

private:
    void publish_frame()
    {
        cv::Mat frame;
        if (!cap_.read(frame)) {
            RCLCPP_WARN(this->get_logger(), "无法读取视频帧");
            return;
        }

        if (frame.empty()) {
            RCLCPP_WARN(this->get_logger(), "读取到空帧");
            return;
        }

        // 将 OpenCV 图像转换为 ROS 图像消息
        std_msgs::msg::Header header;
        header.stamp = this->get_clock()->now();
        header.frame_id = "camera_frame";

        // 使用 cv_bridge 进行转换
        sensor_msgs::msg::Image::SharedPtr img_msg;
        try {
            img_msg = cv_bridge::CvImage(header, "bgr8", frame).toImageMsg();
        }
        catch (cv_bridge::Exception& e) {
            RCLCPP_ERROR(this->get_logger(), "cv_bridge 转换失败: %s", e.what());
            return;
        }

        // 发布图像消息
        image_pub_->publish(*img_msg);
    }

    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    cv::VideoCapture cap_;
    std::string rtsp_url_;
};

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<RTSPStreamer>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
